package edu.arizona.sista.learn.activity.fluents.paths;

import java.util.Vector;

public class Path3D {
	private Vector<Point3D> points;
	
	public Path3D() {
		this(new Vector<Point3D>());
	}
	
	public Path3D(int numPoints) {
		Vector<Point3D> points = new Vector<Point3D>(numPoints);
		for (int i = 0; i < numPoints; i++) {
			points.add(null);
		}
		setPoints(points);
	}
	
	public Path3D(Vector<Point3D> points) {
		setPoints(points);
	}
	
	@Override
	public String toString() {
		return "<Path3D(" + numPoints() + " points)>";
	}

	public void setPoints(Vector<Point3D> points) {
		this.points = points;
	}

	public Vector<Point3D> getPoints() {
		return points;
	}
	
	public int numPoints() {
		return points.size();
	}
	
	public void addPoint(String x, String y, String z) {
		addPoint(Double.parseDouble(x), Double.parseDouble(y), Double.parseDouble(z));
	}
	
	public void addPoint(double x, double y, double z) {
		addPoint(new Point3D(x, y, z));
	}
	
	public void addPoint(Point3D p) {
		points.add(p);
	}
	
	public Point3D getPoint(int i) {
		return points.get(i);
	}
	
	public void setPoint(int i, String x, String y, String z) {
		setPoint(i, Double.parseDouble(x), Double.parseDouble(y), Double.parseDouble(z));
	}
	
	public void setPoint(int i, double x, double y, double z) {
		setPoint(i, new Point3D(x, y, z));
	}
	
	public void setPoint(int i, Point3D p) {
		points.set(i, p);
	}
	
	public void movingAverage(int winsize) {
		if (winsize == 1) {
			return;
		}
		else if (winsize % 2 == 0) {
			// TODO throw exception if window size is multiple of 2
		}
		else if (winsize < 0) {
			// TODO throw exception if window size is negative
		}
		
		int numpoints = points.size();
		
		Vector<Point3D> newPoints = new Vector<Point3D>(numpoints);
		
		for (int i = 0; i < numpoints; i++) {
			if (points.get(i) == null) {
				newPoints.add(null);
				continue;
			}
			
			double cx = 0;
			double cy = 0;
			double cz = 0;
			int n = 0;
			for (int j = i - (winsize/2); j < i + (winsize/2); j++) {
				if (j < 0 || j >= numpoints) {
					continue;
				}
				Point3D p = points.get(j);
				
				if (p == null) {
					continue;
				}
				
				cx += p.getX();
				cy += p.getY();
				cz += p.getZ();
				n++;
			}
			
			newPoints.add(new Point3D(cx/n, cy/n, cz/n));
		}
		
		setPoints(newPoints);
	}
}
